function [] = sendCommand(whichBoat, command, varargin)

defaultSpeed = 1;
defaultPrecision = 5;

assert((whichBoat==1) || (whichBoat==2) || (whichBoat == 3), ...
    'Bad boat selection, needs to be 1, 2, or 3.\n');

switch whichBoat
    case 1
        boat = '192.168.1.101';
    case 2
        boat = '192.168.1.102';
    case 3
        boat = '192.168.1.103';
end

username = 'actuv';
password = 'CMU-actuv';
path = 'boats\ code/New\ Boats\ Code/';
commandPath = 'boats\ code/New\ Boats\ Code/Commands/';


switch command
    case 'goToWaypoint'
        assert(nargin >= 4, 'Args should be (boat, command, lat, lon, [precision, speed]');
        assert(nargin <= 6, 'Args should be (boat, command, lat, lon, [precision, speed]');
        switch nargin
            case 4
                lat = varargin{3};
                lon = varargin{4};
                precision = defaultPrecision;
                speed = defaultSpeed;
                           
            case 5
                lat = varargin{3};
                lon = varargin{4};
                precision = varargin{5};
                speed = defaultSpeed;                
            case 6
                lat = varargin{3};
                lon = varargin{4};
                precision = varargin{5};
                speed = varargin{6};
        end
        fid = fopen('mission.txt', 'w');
        fprintf(fid, 'goToWaypoint\t[%lf, %lf, %lf, %lf]\n', lat, lon, precision, speed);
        sftp_simple_put(boat, username, password, 'mission.txt', commandPath, 'mission.txt');
    case 'goToHeading'
        assert(nargin >= 3, 'Args should be (boat, command, desHeading, (speed)');
        assert(nargin <= 4, 'Args should be (boat, command, desHeading, (speed)');
        switch nargin
            case 3
                desHeading = varargin{3};
                speed = defaultSpeed;
            case 4
                desHeading = varargin{3};
                speed = varargin{4};
        end
        fid = fopen('mission.txt', 'w');
        fprintf(fid, 'goToHeading\t[%lf, %lf]\n', desHeading, speed);
        sftp_simple_put(boat, username, password, 'mission.txt', commandPath, 'mission.txt');
    case 'halt'
        assert(nargin == 3, 'No args for halt.');
        fid = fopen('mission.txt', 'w');
        fprintf(fid, 'halt\n');
        sftp_simple_put(boat, username, password, 'mission.txt', commandPath, 'mission.txt');
end

end
        
        
        
